/* Version 1.0 */
#include "Fixed_Point_Math.h"

extern void putY(uint16);
extern void putX(uint16);
extern void putRoot(uint16);
extern void putAngle(unsigned int);

/********** LONGITUDE RATIO FOR LATITUDE POSITION *****************/
/* This table gives the relation betwen latitude and the longitude-distance 
relation. The format is 16 bits of integer part and 6 bits of fractional
part. The units are meters per minute. The first position contains the ratio
at latitue = 0 degrees, the last position is at 90 degrees.*/
  
const uint32 u32_GPS_LongRatio[91] = {
0x73D3FB0,	0x73CF76E,	0x73C1EAD,	0x73AB57F,	0x738BBFF,	0x7363255,	
0x73318B3,	0x72F6F58,	0x72B368D,	0x7266EA5,	0x7211801,	0x71B330A,	
0x714C037,	0x70DC009,	0x706330A,	0x6FE19D2,	0x6F57502,	0x6EC4547,	
0x6E28B59,	0x6D847F9,	0x6CD7BF5,	0x6C22823,	0x6B64D66,	0x6A9ECAC,	
0x69D06EA,	0x68F9D22,	0x681B060,	0x67341BA,	0x6645251,	0x654E34E,	
0x644F5E5,	0x6348B54,	0x623A4E4,	0x61243E4,	0x60069B2,	0x5EE17B0,	
0x5DB4F4C,	0x5C811FE,	0x5B46145,	0x5A03EAB,	0x58BABC2,	0x576AA24,	
0x5613B74,	0x54B615F,	0x5351D98,	0x51E71DD,	0x5075FF1,	0x4EFE9A2,	
0x4D810C3,	0x4BFD731,	0x4A73ECF,	0x48E4989,	0x474F950,	0x45B501E,	
0x4414FF3,	0x426FAD6,	0x40C52D5,	0x3F15A04,	0x3D6127E,	0x3BA7E62,	
0x39E9FD8,	0x382790C,	0x3660C2F,	0x3495B7B,	0x32C692A,	0x30F377F,	
0x2F1C8C0,	0x2D41F3A,	0x2B63D3D,	0x298251C,	0x279D932,	0x25B5BD9,	
0x23CAF74,	0x21DD667,	0x1FED319,	0x1DFA7F5,	0x1C0576A,	0x1A0E3E9,	
0x1814FE5,	0x1619DD5,	0x141D033,	0x121E978,	0x101EC21,	0xE1DAAE,	
0xC1B79D,	0xA18572,	0x8146AE,	0x60FDD6,	0x40AD6D,	0x2057FA,	
0x0};
    
/*The next value is the latitude-distance constant ratio. It uses the same
format of last table. The value is 1853.2487 meters by minute*/

const uint32 u32_GPS_LatRatio = 0x73D3FB0;


/********** ARCTAN FUNCTION *****************/
/*Normalized arctan function for 0 to 45 degrees with 128 steps
Fisrt position gives 0 degrees and last position gives 45 degres
A SQ2 format is used, so final result mus be divided by 4*/
const uint8 u8_GPS_arctan[129] = {
0x0,	0x2,	0x4,	0x5,	0x7,	0x9,	
0xB,	0xD,	0xE,	0x10,	0x12,	0x14,	
0x15,	0x17,	0x19,	0x1B,	0x1D,	0x1E,	
0x20,	0x22,	0x24,	0x25,	0x27,	0x29,	
0x2A,	0x2C,	0x2E,	0x30,	0x31,	0x33,	
0x35,	0x36,	0x38,	0x3A,	0x3C,	0x3D,	
0x3F,	0x40,	0x42,	0x44,	0x45,	0x47,	
0x49,	0x4A,	0x4C,	0x4D,	0x4F,	0x51,	
0x52,	0x54,	0x55,	0x57,	0x58,	0x5A,	
0x5B,	0x5D,	0x5F,	0x60,	0x62,	0x63,	
0x64,	0x66,	0x67,	0x69,	0x6A,	0x6C,	
0x6D,	0x6F,	0x70,	0x71,	0x73,	0x74,	
0x75,	0x77,	0x78,	0x79,	0x7B,	0x7C,	
0x7D,	0x7F,	0x80,	0x81,	0x83,	0x84,	
0x85,	0x86,	0x88,	0x89,	0x8A,	0x8B,	
0x8C,	0x8E,	0x8F,	0x90,	0x91,	0x92,	
0x93,	0x95,	0x96,	0x97,	0x98,	0x99,	
0x9A,	0x9B,	0x9C,	0x9D,	0x9F,	0xA0,	
0xA1,	0xA2,	0xA3,	0xA4,	0xA5,	0xA6,	
0xA7,	0xA8,	0xA9,	0xAA,	0xAB,	0xAC,	
0xAD,	0xAE,	0xAF,	0xAF,	0xB0,	0xB1,	
0xB2,	0xB3,	0xB4};


extern uUN32 uUN32_latitudeController;
extern uUN32 uUN32_longitudeController;
extern uUN32 uUN32_latitude;
extern uUN32 uUN32_longitude;
extern uUN32 uUN32_temporal1;
extern uUN32 uUN32_temporal2;
extern uUN64 uUN64_temporal3;
extern uUN32 uUN32_distanceY;
extern uUN32 uUN32_distanceX;
extern uint8 u8TableLatitude;

#pragma MESSAGE DISABLE C4000
#pragma MESSAGE DISABLE C4001    
void vfnGPS_DistanceAproximation(void)
{
    uint8 u8_DivisionTimes=0;
    uint8 u8_iteraciones=0;
    uint8 signs=0;          
    
    //Controller 1, measured 2
    
    if( ( uUN32_latitudeController.l != uUN32_latitude.l ) && ( uUN32_longitudeController.l != uUN32_longitude.l ) )   
    {
        if(uUN32_latitudeController.l > uUN32_latitude.l)    //uUN32_temporal1 contains the latitude increment
        {
            uUN32_temporal1.l = uUN32_latitudeController.l - uUN32_latitude.l;
            signs |= 0x80;      //Latitude controller > latitude measured
        }
        else
        {
            uUN32_temporal1.l = uUN32_latitude.l - uUN32_latitudeController.l;
            signs |= 0x40;      //Latitude controller < latitude measured
        }

        uUN32_temporal2.l = u32_GPS_LatRatio;     //The latitude ratio is loaded into uUN32_temporal2

        mMul32x32to32Q16(uUN32_temporal1, uUN32_temporal2, uUN32_distanceY, uUN64_temporal3);
        
        if(uUN32_longitudeController.l > uUN32_longitude.l)    //uUN32_temporal1 contains the latitude increment
        {
            uUN32_temporal1.l = uUN32_longitudeController.l - uUN32_longitude.l;
            signs |= 0x20;      //Longitude controller > longitude measured
        }
        else
        {
            uUN32_temporal1.l = uUN32_longitude.l - uUN32_longitudeController.l;
            signs |= 0x10;      //Longitude controller < longitude measured
        }

        //It is necesary to know the latitude value with precision of one degree. This data should be 
        //calculated during parsing process

        uUN32_temporal2.l=u32_GPS_LongRatio[u8TableLatitude];
                                                       //The longitude ratio is loaded into uUN32_temporal2

        mMul32x32to32Q16(uUN32_temporal1,uUN32_temporal2,uUN32_distanceX,uUN64_temporal3);

        putY(uUN32_distanceY.shorts.s1);        //Put distances on LCD
        putX(uUN32_distanceX.shorts.s1);
        
        
        while(uUN32_distanceX.shorts.s1 > 115 | uUN32_distanceY.shorts.s1 > 115 )
        {
            uUN32_distanceX.shorts.s1 = uUN32_distanceX.shorts.s1>>1;
            uUN32_distanceY.shorts.s1 = uUN32_distanceY.shorts.s1>>1;
            u8_DivisionTimes++;
        }
        //Xold -> uUN32_temporal2.shorts.s1
        //Xnew -> uUN32_temporal2.shorts.s0
        //
        
        if(uUN32_distanceX.bytes.b2>uUN32_distanceY.bytes.b2)
        {
            uUN32_temporal2.shorts.s1 = uUN32_distanceX.bytes.b2;
            uUN64_temporal3.bytes.b0 = (uint8)(( uUN32_distanceY.bytes.b2<<7)/uUN32_distanceX.bytes.b2); 
                //<<8 is not really computed. Compiler takes low part an puts it on high part before divide
            signs |= 0x08;
        }
        else
        {
            uUN32_temporal2.shorts.s1 = uUN32_distanceY.bytes.b2;
            uUN64_temporal3.bytes.b0 = (uint8)(( uUN32_distanceX.bytes.b2<<7)/uUN32_distanceY.bytes.b2); 
            signs |= 0x04;
          
        }
        
        uUN64_temporal3.bytes.b1=0;
        
        uUN32_temporal1.shorts.s0 = uUN32_distanceX.bytes.b2 * uUN32_distanceX.bytes.b2;
        uUN32_temporal1.shorts.s1 = uUN32_distanceY.bytes.b2 * uUN32_distanceY.bytes.b2;
        
        uUN32_temporal1.shorts.s0 += uUN32_temporal1.shorts.s1;
        
        
        while(1)
        {
            uUN32_temporal2.shorts.s0 = uUN32_temporal1.shorts.s0/uUN32_temporal2.shorts.s1;
            uUN32_temporal2.shorts.s0 += uUN32_temporal2.shorts.s1;
            uUN32_temporal2.shorts.s0 = uUN32_temporal2.shorts.s0>>1;
            
            if(u8_iteraciones == 2)
                break;
            else
                u8_iteraciones++;
            
            uUN32_temporal2.shorts.s1 = uUN32_temporal2.shorts.s0;
        }
        
        uUN64_temporal3.shorts.s1 = u8_GPS_arctan[uUN64_temporal3.bytes.b0];
        
        if(signs & 0x08)
        {
            uUN64_temporal3.shorts.s1 = 360 - uUN64_temporal3.shorts.s1;
        }
        
        if(signs & 0x40)
        {
            if(signs & 0x20)
            {
                
            }
            else
            {
                uUN64_temporal3.shorts.s1 = 1440 - uUN64_temporal3.shorts.s1;
            }
        }
        else
        {
            if(signs & 0x20)
            {
                uUN64_temporal3.shorts.s1 = 720 - uUN64_temporal3.shorts.s1;
            }
            else
            {
                uUN64_temporal3.shorts.s1 = uUN64_temporal3.shorts.s1 + 720;
            }
        }
        
        putAngle(uUN64_temporal3.shorts.s1);
        
        
        uUN32_temporal2.shorts.s0 = uUN32_temporal2.shorts.s0 << u8_DivisionTimes;
        putRoot(uUN32_temporal2.shorts.s0);
    }
    else
    {
        putY(0);        //Put distances on LCD
        putX(0);
        putRoot(0);
        putAngle(0);
    }    
}

